from PyQt5.QtCore import QObject, QTimer, pyqtSignal
import math
import logging

class DroneSimulator(QObject):
    pose_updated = pyqtSignal(dict)
    status_updated = pyqtSignal(str)

    def __init__(self):
        super().__init__()
        self.logger = logging.getLogger("FlightCtrl.Simulator")
        self.running = False
        self.pose = {
            'x': 0.0,
            'y': 0.0,
            'z': 0.0,
            'roll': 0.0,
            'pitch': 0.0,
            'yaw': 0.0,
            'vx': 0.0,
            'vy': 0.0,
            'vz': 0.0
        }
        self.controls = {
            'throttle': 50,
            'yaw': 0,
            'pitch': 0,
            'roll': 0,
            'flight_mode': 'Manual'
        }
        self.timer = QTimer(self)
        self.timer.timeout.connect(self.simulate_step)
        self.simulation_speed = 1.0  # 1x real time
        self.gravity = 9.81  # m/s²
        self.mass = 1.2  # kg
        self.max_thrust = 20.0  # N
        self.drag_coefficient = 0.1

    def start(self):
        self.running = True
        self.timer.start(16)  # ~60 FPS
        self.status_updated.emit("Simulation started")

    def stop(self):
        self.running = False
        self.timer.stop()
        self.status_updated.emit("Simulation stopped")

    def is_running(self):
        return self.running

    def set_control(self, controls):
        self.controls.update(controls)
        if self.controls['flight_mode'] == 'Emergency':
            self.status_updated.emit("EMERGENCY STOP ACTIVATED")

    def simulate_step(self):
        dt = 0.016 * self.simulation_speed  # 时间步长
        
        # 计算推力
        throttle = self.controls['throttle'] / 100.0
        thrust = self.max_thrust * throttle
        
        # 计算姿态变化
        self.pose['yaw'] += math.radians(self.controls['yaw'] * 0.1)
        self.pose['pitch'] = math.radians(self.controls['pitch'] * 0.5)
        self.pose['roll'] = math.radians(self.controls['roll'] * 0.5)
        
        # 计算加速度
        ax = (thrust * math.sin(self.pose['pitch']) / self.mass) 
        ax -= self.drag_coefficient * self.pose['vx']
        
        ay = (thrust * math.sin(self.pose['roll']) / self.mass) 
        ay -= self.drag_coefficient * self.pose['vy']
        
        az = (thrust * math.cos(self.pose['pitch']) * math.cos(self.pose['roll']) / self.mass) 
        az -= self.gravity
        az -= self.drag_coefficient * self.pose['vz']
        
        # 更新速度
        self.pose['vx'] += ax * dt
        self.pose['vy'] += ay * dt
        self.pose['vz'] += az * dt
        
        # 更新位置
        self.pose['x'] += self.pose['vx'] * dt
        self.pose['y'] += self.pose['vy'] * dt
        self.pose['z'] += self.pose['vz'] * dt
        
        # 限制高度不低于地面
        if self.pose['z'] < 0:
            self.pose['z'] = 0
            self.pose['vz'] = 0
        
        # 发送状态更新
        self.pose_updated.emit({
            'x': self.pose['x'],
            'y': self.pose['y'],
            'z': self.pose['z'],
            'roll': math.degrees(self.pose['roll']),
            'pitch': math.degrees(self.pose['pitch']),
            'yaw': math.degrees(self.pose['yaw']) % 360
        })
        
        # 日志记录
        self.logger.debug(f"Pose: {self.pose}")